#include "StdAfx.h"
#include <math.h>
#include "time.h"
#include "VocoderImpl.h"

extern "C"
{
#include "../VocoderBase/vocode.h"
}

VocoderImpl::VocoderImpl(void)
{
}

VocoderImpl::~VocoderImpl(void)
{
	free_vocoder_memory();
}

void VocoderImpl::Initialize(int windowLength, int bandCount, double volume, bool normalize, int samplingRate)
{
	this->windowLength = windowLength;
	this->bandCount = bandCount;
	this->volume = volume;
	this->normalize = normalize;
	this->samplingRate = samplingRate;

	vocode_window_length = windowLength;
	set_default_max_magnitude();
	allocate_memory2();
}

static void copyToDouble(short* shortAr, int length, std::vector<double>& dVec)
{
	dVec.resize(length);
	for(int i=0; i<length; i++)
	{
		dVec[i] = (double)shortAr[i];
	}
}

static std::vector<long> fftProcessingTime;
static std::vector<long> vocodeProcessingTime;
void VocoderImpl::Vocode(short* modulator, short* carrier, Sample& vocoded)
{
	long fftStart = clock();
	/*
	std::vector<double> dModulator;
	copyToDouble(modulator, windowLength, dModulator);
	fft->FFTMagnitude(dModulator);

	std::vector<double> dCarrierReal;
	std::vector<double> dCarrierImag(windowLength, 0);
	copyToDouble(carrier, windowLength, dCarrierReal);
	fft->DoFFT(dCarrierReal, dCarrierImag);
	fft->NormalizeFFT(dCarrierReal, dCarrierImag);
	long fftFinish = clock();
	fftProcessingTime.push_back(fftFinish-fftStart);

	long vocodeStart = clock();
	std::vector<double> dVocodedReal;
	std::vector<double> dVocodedImag;
	VocodeFFTdWindow(dModulator, dCarrierReal, dCarrierImag, dVocodedReal, dVocodedImag);
	fft->InvFFT(dVocodedReal, dVocodedImag);
	doubleToSample(dVocodedReal, vocoded, SHORT_MAX, volume);
	*/


	long vocodeStart = clock();
	vocode_window_sample(modulator, carrier, vocoded.GetData(), NULL);
	long vocodeFinish = clock();
	vocodeProcessingTime.push_back(vocodeFinish-vocodeStart);
}

void VocoderImpl::VocodeFFTdWindow(std::vector<double>& dModulator, 
					  std::vector<double>& dCarrierReal, 
					  std::vector<double>& dCarrierImag, 
					  std::vector<double>& dVocodedReal, 
					  std::vector<double>& dVocodedImag)
{
	dVocodedReal.resize(windowLength);
	dVocodedImag.resize(windowLength);
  int bandNo, bandLength, extraBandLength;
  bandLength = windowLength / (bandCount * 2);
  extraBandLength = windowLength / 2 - bandLength * (bandCount - 1);

  for (bandNo = 0; bandNo < bandCount; bandNo++) {
    int i, j, k, l;
    double m, c;
    
    l = (bandNo == bandCount - 1) ? extraBandLength : bandLength;

    m = 0; c = 0;
    for (i = 0, j = bandNo * bandLength, k = windowLength - j - 1;
         i < l; i++, j++, k--)
      {
        if (normalize) {
          double c1 = dCarrierReal[j]*dCarrierReal[j] + dCarrierImag[j]*dCarrierImag[j],
               c2 = dCarrierReal[k]*dCarrierReal[k] + dCarrierImag[k]*dCarrierImag[k];
          c += sqrt(c1) + sqrt(c2);
        }
        m += dModulator[j];
      }

    if (!normalize) c = 1.0;
    if (c == 0) c = 0.0001;

    for (i = 0, j = bandNo * bandLength, k = windowLength - j - 1;
         i < l; i++, j++, k--) {
      dVocodedReal[j] = dCarrierReal[j] * m / c;
      dVocodedImag[j] = dCarrierImag[j] * m / c;
      dVocodedReal[k] = dCarrierReal[k] * m / c;
      dVocodedImag[k] = dCarrierImag[k] * m / c;
    }
  }
}
